
#include "CDisplay.h"
#include <GL/glut.h>
#include <XnCppWrapper.h>
#include "utils.h"

/*


General way this works:
  CSkeleton S is continuously fed new points from the tracker system.
	S will add the points it wants to appropriate vectors IFF the confidence is high 
		--Might need to add them anyway, as placeholders, so other points line up at same time
	At set intervals, S will evaluate the current points, and determine smoothness, etc.
	    (eval and return from S.EvalPoints() as a vector of doubles)
	These values are passed to the CPlayer P, which will use them to set up the next measure

	P.Play() should only be called on the actual measure point.


*/



CDisplay::CDisplay(int win_x, int win_y, DepthGenerator* D, DepthMetaData* DMD)
{
	//Set the window parameters
	mX = win_x;
	mY = win_y;
	valid = false;
	minConf = 0.7; //Might need to adjust this. Let it just run and see what's printed
	S.setConf(minConf);
	S.SetWin(mX, mY);
	depth = D;
	depthMD = DMD;
	count = 0;
}

CDisplay::~CDisplay()
{
	//Clear everything
}

void CDisplay::LostUser(XnUserID I)
{
	//Only make it not valid if the user ID that was lost is the current user
	if (I == uID)
		valid = false;
	//Anything else?
}

void CDisplay::ScalePoint(XnPoint3D& point)
{
	point.X *= mX;
	point.X /= depthMD->XRes();

	point.Y *= mY;
	point.Y /= depthMD->YRes();
}

void CDisplay::EvalSkeleton()
{
	//
	/*typedef enum XnSkeletonJoint
{
	XN_SKEL_HEAD			= 1, // For position relative to torso? for minor consideration? not comf
	XN_SKEL_NECK			= 2,
	XN_SKEL_TORSO			= 3,
	XN_SKEL_WAIST			= 4,

	XN_SKEL_LEFT_COLLAR		= 5,
	XN_SKEL_LEFT_SHOULDER	= 6,
	XN_SKEL_LEFT_ELBOW		= 7,
	XN_SKEL_LEFT_WRIST		= 8,
	XN_SKEL_LEFT_HAND		= 9,
	XN_SKEL_LEFT_FINGERTIP	=10, // Only if confidence is high... comparison with hand and wrist?

	XN_SKEL_RIGHT_COLLAR	=11,
	XN_SKEL_RIGHT_SHOULDER	=12,
	XN_SKEL_RIGHT_ELBOW		=13,
	XN_SKEL_RIGHT_WRIST		=14,
	XN_SKEL_RIGHT_HAND		=15,
	XN_SKEL_RIGHT_FINGERTIP	=16,

	XN_SKEL_LEFT_HIP		=17, // Don't deal with this yet
	XN_SKEL_LEFT_KNEE		=18,
	XN_SKEL_LEFT_ANKLE		=19,
	XN_SKEL_LEFT_FOOT		=20,

	XN_SKEL_RIGHT_HIP		=21,
	XN_SKEL_RIGHT_KNEE		=22,
	XN_SKEL_RIGHT_ANKLE		=23,
	XN_SKEL_RIGHT_FOOT		=24	
} XnSkeletonJoint;*/
	if (valid && user.GetSkeletonCap().IsTracking(uID))
	{
		//At this point we know that the user skeleton is being tracked. 
		XnSkeletonJointPosition RH, LH, H, T;
		user.GetSkeletonCap().GetSkeletonJointPosition( uID, XN_SKEL_RIGHT_HAND, RH); 
		user.GetSkeletonCap().GetSkeletonJointPosition( uID, XN_SKEL_LEFT_HAND, LH); 
		user.GetSkeletonCap().GetSkeletonJointPosition( uID, XN_SKEL_HEAD, H); 
		user.GetSkeletonCap().GetSkeletonJointPosition( uID, XN_SKEL_TORSO, T); 

		XnPoint3D	point = RH.position;
		depth->ConvertRealWorldToProjective(1, &point, &point);
		ScalePoint(point);
		XnSkeletonJointPosition RHmod;
		RHmod.position = point;
		RHmod.fConfidence = RH.fConfidence;
		S.addpt(XN_SKEL_RIGHT_HAND, RHmod); 

		point = LH.position;
		depth->ConvertRealWorldToProjective(1, &point, &point);
		ScalePoint(point);
		XnSkeletonJointPosition LHmod;
		LHmod.position = point;
		LHmod.fConfidence = LH.fConfidence;
		S.addpt(XN_SKEL_LEFT_HAND, LHmod); 

		point = H.position;
		depth->ConvertRealWorldToProjective(1, &point, &point);
		ScalePoint(point);
		XnSkeletonJointPosition Hmod;
		Hmod.position = point;
		Hmod.fConfidence = H.fConfidence;
		S.addpt(XN_SKEL_HEAD, Hmod); 

		point = T.position;
		depth->ConvertRealWorldToProjective(1, &point, &point);
		ScalePoint(point);
		XnSkeletonJointPosition Tmod;
		Tmod.position = point;
		Tmod.fConfidence = T.fConfidence;
		S.addpt(XN_SKEL_TORSO, Tmod); 
	}

	count++;
	if (count >= 5 || !P.IsPlaying()) 
	{
		std::vector<float> attrs = S.EvalPts();
		P.SetAttrs(attrs);
		P.SetProgMode(S.IsProgMode());
		count = 0;
	}
}

bool CDisplay::Draw()
{
	//First, evaluate the current skeleton. If it gets slow, set up how often this should occur.
	EvalSkeleton();

	glDisable(GL_LIGHTING);//TEXTURE_2D);
	glDisable(GL_TEXTURE_2D);


	S.Draw(); //Drawing the skeleton
	if (valid && user.GetSkeletonCap().IsTracking(uID))//Only play if the user's actually being tracked
		P.Play();
	P.Draw(mX, mY);
	

	glEnable(GL_TEXTURE_2D);

	return true;
}


bool CDisplay::DrawSkeleton()
{
	/*typedef enum XnSkeletonJoint
		{
			XN_SKEL_HEAD			= 1,
			XN_SKEL_NECK			= 2,
			XN_SKEL_TORSO			= 3,
			XN_SKEL_WAIST			= 4,

			XN_SKEL_LEFT_COLLAR		= 5,
			XN_SKEL_LEFT_SHOULDER	= 6,
			XN_SKEL_LEFT_ELBOW		= 7,
			XN_SKEL_LEFT_WRIST		= 8,
			XN_SKEL_LEFT_HAND		= 9,
			XN_SKEL_LEFT_FINGERTIP	=10,

			XN_SKEL_RIGHT_COLLAR	=11,
			XN_SKEL_RIGHT_SHOULDER	=12,
			XN_SKEL_RIGHT_ELBOW		=13,
			XN_SKEL_RIGHT_WRIST		=14,
			XN_SKEL_RIGHT_HAND		=15,
			XN_SKEL_RIGHT_FINGERTIP	=16,

			XN_SKEL_LEFT_HIP		=17,
			XN_SKEL_LEFT_KNEE		=18,
			XN_SKEL_LEFT_ANKLE		=19,
			XN_SKEL_LEFT_FOOT		=20,

			XN_SKEL_RIGHT_HIP		=21,
			XN_SKEL_RIGHT_KNEE		=22,
			XN_SKEL_RIGHT_ANKLE		=23,
			XN_SKEL_RIGHT_FOOT		=24	
		} XnSkeletonJoint;*/
	if (valid && user.GetSkeletonCap().IsTracking(uID))
	{
		//At this point we know that the user skeleton is being tracked. 
		XnSkeletonJointPosition Head; 
		user.GetSkeletonCap().GetSkeletonJointPosition( uID, XN_SKEL_HEAD, Head); 
		printf("%d: (%f,%f,%f) [%f]\n", uID, Head.position.X, Head.position.Y, Head.position.Z, Head.fConfidence);
		return true;
	}

	return false;
}
